#include "drv_main.h"

/**********************************************************************************************************

             麦克纳姆轮安装方向

        A轮  \\    ------   //  B轮
			  \\  ------  //
				  ------
				  ------				
				  ------
			 //  ------  \\
	   D轮  //   ------   \\  C轮
				
			//四驱底盘及四轮麦克纳姆轮底盘
			//硬件连接说明：
			timer4 控制ABCD四个轮子的PWM
			TIM4_CH1--PB6--A轮 //左前 ---- 电机驱动1-ENA
			TIM4_CH2--PB7--B轮 //右前 ---- 电机驱动1-ENB
			TIM4_CH3--PB8--C轮 //右后 ---- 电机驱动2-ENA
		    TIM4_CH4--PB9--D轮 //左后 ---- 电机驱动2-ENB
			
			
			A轮：PA6 PA7 控制前后运动   PA7----IN2，PA6----IN1
			B轮：PB0 PB1 控制前后运动   PB0----IN4，PB1----IN3			
			C轮：PB3，PA15控制前后运动  PB3 --- IN1，PA15---IN2
			D轮：PB4，PB5控制前后运动   PB4 --- IN3，PB5 ---IN4
					    
**********************************************************************************************************
*/

static void Drv_Motor_Dir_Config(void)
{
	//重映射需要先使能AFIO时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO ,ENABLE);
	/*PA15/JTDI、PB3/JTDO、PB4/JNTRST；这几个管脚复位后的主功能是JTDI 、JDO、 JNTRST。要这几个管脚作为普通I/O口用时，在初始化配置时的前面添加*/
	GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); 

	/*定义一个GPIO_InitTypeDef类型的结构体*/
	GPIO_InitTypeDef GPIO_InitStructure;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);						/*开启GPIO的外设时钟*/																   
	GPIO_InitStructure.GPIO_Pin =  MOTOR_AIN2_PIN | MOTOR_CIN2_PIN;				/*选择要控制的GPIO引脚*/	
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;                         	/*设置引脚模式为通用推挽输出*/   
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;                          	/*设置引脚速率为50MHz */ 
	GPIO_Init(MOTOR_AIN2_PORT, &GPIO_InitStructure); 							/*调用库函数，初始化GPIO*/	

	RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB, ENABLE);						/*开启GPIO的外设时钟*/																   
	GPIO_InitStructure.GPIO_Pin = MOTOR_AIN1_PIN | MOTOR_BIN1_PIN| MOTOR_BIN2_PIN | MOTOR_CIN1_PIN | MOTOR_DIN1_PIN | MOTOR_DIN2_PIN;	/*选择要控制的GPIO引脚*/	
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;                              /*设置引脚模式为通用推挽输出*/   
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;                             /*设置引脚速率为50MHz */ 
	GPIO_Init(MOTOR_BIN1_PORT, &GPIO_InitStructure);    
}

static void Drv_Motor_Config(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
	
	GPIO_InitStructure.GPIO_Pin=PWM1_PIN;
	GPIO_Init(PWM1_PORT,&GPIO_InitStructure);

	GPIO_InitStructure.GPIO_Pin=PWM2_PIN;
	GPIO_Init(PWM2_PORT,&GPIO_InitStructure);

	GPIO_InitStructure.GPIO_Pin=PWM3_PIN;
	GPIO_Init(PWM3_PORT,&GPIO_InitStructure);

	GPIO_InitStructure.GPIO_Pin=PWM4_PIN;
	GPIO_Init(PWM4_PORT,&GPIO_InitStructure);
		
	
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
	TIM_OCInitTypeDef TIM_OCInitStructure;
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
	
	TIM_TimeBaseStructure.TIM_Period=TIM4_PWM_FRELOAD;
	TIM_TimeBaseStructure.TIM_Prescaler=TIM4_PWM_PRESCALER;
	TIM_TimeBaseStructure.TIM_ClockDivision=TIM4_PWM_DIVISION;
	TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up;
	TIM_TimeBaseInit(MOTOR_TIME,&TIM_TimeBaseStructure);
	
	TIM_OCInitStructure.TIM_OCMode=TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OCPolarity=TIM_OCPolarity_High;
	TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_Low;
	TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;
	TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset;	  
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;

	
	TIM_OCInitStructure.TIM_OutputState=TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse=0;
	TIM_OC1Init(MOTOR_TIME,&TIM_OCInitStructure);
	TIM_OC1PreloadConfig(MOTOR_TIME,TIM_OCPreload_Enable);
	
	TIM_OCInitStructure.TIM_OutputState=TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse=0;
	TIM_OC2Init(MOTOR_TIME,&TIM_OCInitStructure);
	TIM_OC2PreloadConfig(MOTOR_TIME,TIM_OCPreload_Enable);

	TIM_OCInitStructure.TIM_OutputState=TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse=0;
	TIM_OC3Init(MOTOR_TIME,&TIM_OCInitStructure);
	TIM_OC3PreloadConfig(MOTOR_TIME,TIM_OCPreload_Enable);
	
	TIM_OCInitStructure.TIM_OutputState=TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse=0;
	TIM_OC4Init(MOTOR_TIME,&TIM_OCInitStructure);
	TIM_OC4PreloadConfig(MOTOR_TIME,TIM_OCPreload_Enable);
	
	TIM_ARRPreloadConfig(MOTOR_TIME,ENABLE);                       
	TIM_Cmd(MOTOR_TIME,ENABLE);	 
	TIM_CtrlPWMOutputs(MOTOR_TIME, ENABLE);	
}

void Drv_wirte_motor_duty(uint8_t id ,uint16_t duty)
{
		switch(id)
		{
			case MOTOR_A:
				TIM_SetCompare1(MOTOR_TIME,duty);
			break;
				
			case MOTOR_B:
				TIM_SetCompare2(MOTOR_TIME,duty);
			break;
				
			case MOTOR_C:
				TIM_SetCompare3(MOTOR_TIME,duty);
			break;
				
			case MOTOR_D:
				TIM_SetCompare4(MOTOR_TIME,duty);
			break;
		}
}

void Drv_wirte_motor_dir(uint8_t id,uint8_t dir_state)
{
	switch(id)
	{
		case MOTOR_A:
			if(dir_state == GO)
			{
				GPIO_SetBits(MOTOR_AIN1_PORT,MOTOR_AIN1_PIN);
				GPIO_ResetBits(MOTOR_AIN2_PORT,MOTOR_AIN2_PIN);
			}
			else if(dir_state == BACK)
			{
				GPIO_ResetBits(MOTOR_AIN1_PORT,MOTOR_AIN1_PIN);
				GPIO_SetBits(MOTOR_AIN2_PORT,MOTOR_AIN2_PIN);
			}
			else if(dir_state == STOP)
			{
				GPIO_ResetBits(MOTOR_AIN1_PORT,MOTOR_AIN1_PIN);
				GPIO_ResetBits(MOTOR_AIN2_PORT,MOTOR_AIN2_PIN);
			}
		break;
			
		case MOTOR_B:
			if(dir_state == GO)
			{
				GPIO_SetBits(MOTOR_BIN1_PORT,MOTOR_BIN1_PIN);
				GPIO_ResetBits(MOTOR_BIN2_PORT,MOTOR_BIN2_PIN);
			}
			else if(dir_state == BACK)
			{
				GPIO_ResetBits(MOTOR_BIN1_PORT,MOTOR_BIN1_PIN);
				GPIO_SetBits(MOTOR_BIN2_PORT,MOTOR_BIN2_PIN);
			}
			else if(dir_state == STOP)
			{
				GPIO_ResetBits(MOTOR_BIN1_PORT,MOTOR_BIN1_PIN);
				GPIO_ResetBits(MOTOR_BIN2_PORT,MOTOR_BIN2_PIN);
			}
		break;
			
		case MOTOR_C:
			if(dir_state == GO)
			{
				GPIO_ResetBits(MOTOR_CIN1_PORT,MOTOR_CIN1_PIN);
				GPIO_SetBits(MOTOR_CIN2_PORT,MOTOR_CIN2_PIN);
			}
			else if(dir_state == BACK)
			{
				GPIO_SetBits(MOTOR_CIN1_PORT,MOTOR_CIN1_PIN);
				GPIO_ResetBits(MOTOR_CIN2_PORT,MOTOR_CIN2_PIN);
			}
			else if(dir_state == STOP)
			{
				GPIO_ResetBits(MOTOR_CIN1_PORT,MOTOR_CIN1_PIN);
				GPIO_ResetBits(MOTOR_CIN2_PORT,MOTOR_CIN2_PIN);
			}
		break;
			
		case MOTOR_D:
			if(dir_state == GO)
			{
				GPIO_ResetBits(MOTOR_DIN1_PORT,MOTOR_DIN1_PIN);
				GPIO_SetBits(MOTOR_DIN2_PORT,MOTOR_DIN2_PIN);
			}
			else if(dir_state == BACK)
			{
				GPIO_SetBits(MOTOR_DIN1_PORT,MOTOR_DIN1_PIN);
				GPIO_ResetBits(MOTOR_DIN2_PORT,MOTOR_DIN2_PIN);
			}
			else if(dir_state == STOP)
			{
				GPIO_ResetBits(MOTOR_DIN1_PORT,MOTOR_DIN1_PIN);
				GPIO_ResetBits(MOTOR_DIN2_PORT,MOTOR_DIN2_PIN);
			}
		break;
		
	}
}


/**
* @brief  电机驱动初始化
	* @parm   None
	* @retval None
	*/
void Drv_Motor_Init(void)
{
	Drv_Motor_Config();
	Drv_Motor_Dir_Config();
	
	Dev_Motor_Init();
}

